#include "main.h"
uint16_t data1_[16];

int main(void)
{
	Att_Angle.yaw = 0.0;
	delay_init(168); 
	LED_Init();//LED指示灯
	delay_ms(1000);//延时后开始初始化
	NVIV_Config();//配置中断优先级
	MOTOR_Init();
	ESC_Init();
	
	IIC_Init();
	BMI088_Init(); 	
	HMC58831L_Init(); 		
//	SPL06_Init();//气压计初始化
	PPM_Init();//PPM解码初始化
	Offset_Read();
	Usart1_Init(115200);//上位机通信
//	Usart4_Init(115200);//MAVLink协议通信 
	TIM2_Init();//计时时间控制总线 + 传感器采样与控制时间总线
	printf("esc init successful\n");
	Filter_Init();//滤波器初始化
	printf("init successful\n");

	
	bool flag_arming = false;
	bool flag_arming1 = false;
	bool flag_arming2 = false;
	uint16_t s_t;
	uint32_t t1;
	uint32_t t2;
	while(1)
	{

				//printf("pit_des:\t%f\tpit_mea:\t%f\tPID:\t%f\n",Pit_Aim,Att_Angle.pit,PID_PIT_R.OutPut);
				//printf("rol_des:\t%f\trol_mea:\t%f\tPID:\t%f\n",Rol_Aim,Att_Angle.rol,PID_ROL_R.OutPut);
				printf("yaw_des:\t%f\tyaw_mea:\t%f\tPID:\t%f\n\n",init_yaw,kalman_yaw,Att_Angle.yaw);
				delay_ms(300);
		////内八字解锁
		if(PPM_Databuf[2]<1200&&PPM_Databuf[3]>1800&&flag_arming1==false)
		{	
			flag_arming1=true;
			t1 = ms10;
		}
		if(flag_arming1==true)
		{
			if(PPM_Databuf[2]<1200&&PPM_Databuf[3]>1800)
			{
				if(ms10-t1>200)
				{
					flag_arming1=false;
					UAV_Arming = true;
					printf("UAV_Arming");
				}
			}else flag_arming1=false;
		}
				
		//外八字上锁
		if(PPM_Databuf[2]<1200&&PPM_Databuf[3]<1200&&flag_arming2==false)
		{	
			flag_arming2=true;
			t2 = ms10;
		}
		if(flag_arming2==true)
		{
			if(PPM_Databuf[2]<1200&&PPM_Databuf[3]<1200)
			{
				if(ms10-t2>200)
				{
					flag_arming2=false;
					UAV_Arming = false;
					printf("UAV_Disrming");
				}
			}
			else flag_arming2=false;
		}
		
		//读取摇杆变化
		for( uint8_t i=0;i<8;i++ ){
			dif_PPM_Databuf[i] = PPM_Databuf[i] - Last_PPM_Databuf[i];
		}
		
		//摇杆长时间无变化,无人机上锁
		//检测到停止打杆
		if(dif_PPM_Databuf[0]<20&&dif_PPM_Databuf[1]<20&&dif_PPM_Databuf[2]<20&&dif_PPM_Databuf[3]<20&&PPM_Databuf[2]<1100&&flag_arming==false)
		{
			flag_arming = true;
			s_t = ms10;
		}
		if(flag_arming == true)
		{
			if(dif_PPM_Databuf[0]<20&&dif_PPM_Databuf[1]<20&&dif_PPM_Databuf[2]<20&&dif_PPM_Databuf[3]<20&&PPM_Databuf[2]<1100)
			{
				if(ms10-s_t>1500)//超过15s摇杆都没变化了
				{
					UAV_Arming = false;
					printf("UAV_Disarming!");
					flag_arming = false;
				}
			}
			else
			{
				flag_arming = false;//打杆了
			}
		}
	}
}




